// Copyright 2008 Isis Innovation Limited
#include "MiniPatch.h"

namespace PTAM {

using namespace CVD;
using namespace std;

// Scoring function
inline int MiniPatch::SSDAtPoint(CVD::BasicImage <CVD::byte>& im, const CVD::ImageRef& ir) {
    if (!im.in_image_with_border(ir, mnHalfPatchSize))
        return mnMaxSSD + 1;
    ImageRef irImgBase = ir - ImageRef(mnHalfPatchSize, mnHalfPatchSize);
    int nRows = mimOrigPatch.size().y;
    int nCols = mimOrigPatch.size().x;
    byte* imagepointer;
    byte* templatepointer;
    int nDiff;
    int nSumSqDiff = 0;
    for (int nRow = 0; nRow < nRows; nRow++) {
        imagepointer = &im[irImgBase + ImageRef(0, nRow)];
        templatepointer = &mimOrigPatch[ImageRef(0, nRow)];
        for (int nCol = 0; nCol < nCols; nCol++) {
            nDiff = imagepointer[nCol] - templatepointer[nCol];
            nSumSqDiff += nDiff * nDiff;
        };
    };
    return nSumSqDiff;
}

// Find a patch by searching at FAST corners in an input image
// If available, a row-corner LUT is used to speed up search through the
// FAST corners
bool MiniPatch::FindPatch(CVD::ImageRef& irPos,
                          CVD::BasicImage <CVD::byte>& im,
                          int nRange,
                          vector<ImageRef>& vCorners,
                          std::vector<int>* pvRowLUT) {
    ImageRef irCenter = irPos;
    ImageRef irBest;
    int nBestSSD = mnMaxSSD + 1;
    ImageRef irBBoxTL = irPos - ImageRef(nRange, nRange);
    ImageRef irBBoxBR = irPos + ImageRef(nRange, nRange);
    vector<ImageRef>::iterator i;
    if (!pvRowLUT) {
        for (i = vCorners.begin(); i != vCorners.end(); i++)
            if (i->y >= irBBoxTL.y) break;
    }
    else {
        int nTopRow = irBBoxTL.y;
        if (nTopRow < 0)
            nTopRow = 0;
        if (nTopRow >= (int) pvRowLUT->size())
            nTopRow = (int) pvRowLUT->size() - 1;
        i = vCorners.begin() + (*pvRowLUT)[nTopRow];
    }

    for (; i != vCorners.end(); i++) {
        if (i->x < irBBoxTL.x || i->x > irBBoxBR.x)
            continue;
        if (i->y > irBBoxBR.y)
            break;
        int nSSD = SSDAtPoint(im, *i);

        if (nSSD < nBestSSD) {
            irBest = *i;
            nBestSSD = nSSD;
        }
    }
    if (nBestSSD < mnMaxSSD) {
        irPos = irBest;
        return true;
    }
    else
        return false;
}

// Define the patch from an input image
void MiniPatch::SampleFromImage(ImageRef irPos, BasicImage <byte>& im) {
    assert(im.in_image_with_border(irPos, mnHalfPatchSize));
    CVD::ImageRef irPatchSize(2 * mnHalfPatchSize + 1, 2 * mnHalfPatchSize + 1);
    mimOrigPatch.resize(irPatchSize);
    copy(im, mimOrigPatch, mimOrigPatch.size(), irPos - mimOrigPatch.size() / 2);
}

// Static members
int MiniPatch::mnHalfPatchSize = 4;
int MiniPatch::mnRange = 10;
int MiniPatch::mnMaxSSD = 9999;

}
